{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "05124c35",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/robot.html).  I recommend having both windows open, side-by-side!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99268d0c",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu",
    "lines_to_end_of_cell_marker": 2
   },
   "outputs": [],
   "source": [
    "from pydrake.all import ModelVisualizer, StartMeshcat, PackageMap\n",
    "\n",
    "from manipulation import running_as_notebook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7dec0a5e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "acb11ff1",
   "metadata": {
    "colab_type": "text",
    "id": "wJWL-ldv5REK"
   },
   "source": [
    "# Robot arms\n",
    "\n",
    "The next two cells will give you a simple interface to move the joints around on a variety of robot arm models.  I highly recommend opening the \"controls\" menu in the meshcat visualizer. You can expand the tree under \"drake\", then check and uncheck the different properties (illustration / inertia / proximity) to explore the model (and the visualization tool).\n",
    "\n",
    "Have a favorite robot that I haven't included here?  If you send me a link to a properly licensed URDF or SDF description of the robot and it's mesh files, I'm happy to add it!  It's worth taking a look at the files quickly, to get a sense for how they work: [SDF](https://github.com/RobotLocomotion/drake/blob/master/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf), [URDF](https://github.com/RobotLocomotion/drake/blob/master/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf).\n",
    "\n",
    "Note: The Jaco doesn't visualize properly in this renderer yet.  See drake issue [#13846](https://github.com/RobotLocomotion/drake/issues/13846)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c11c5ea",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "9GSF52A8ExQa"
   },
   "outputs": [],
   "source": [
    "# First pick your robot by un-commenting one of these:\n",
    "robot = \"Kuka LBR iiwa 7\"\n",
    "# robot = \"Kuka LBR iiwa 14\"\n",
    "# robot = \"Kinova Jaco Gen2 (7 DoF)\"\n",
    "# robot = \"Franka Emika Panda\"\n",
    "# robot = \"UR3e\"\n",
    "\n",
    "\n",
    "def get_model_url(description):\n",
    "    # Note: I could download remote model resources here if necessary.\n",
    "    if description == \"Kuka LBR iiwa 7\":\n",
    "        return \"package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf\"\n",
    "    elif description == \"Kuka LBR iiwa 14\":\n",
    "        return \"package://drake/manipulation/models/iiwa_description/urdf/iiwa14_primitive_collision.urdf\"\n",
    "    elif description == \"Kinova Jaco Gen2 (7 DoF)\":\n",
    "        return \"package://drake/manipulation/models/jaco_description/urdf/j2s7s300.urdf\"\n",
    "    elif description == \"Franka Emika Panda\":\n",
    "        return \"package://drake/manipulation/models/franka_description/urdf/panda_arm_hand.urdf\"\n",
    "    elif description == \"UR3e\":\n",
    "        return \"package://drake/manipulation/models/ur3e/ur3e_cylinders_collision.urdf\"\n",
    "    raise Exception(\"Unknown model\")\n",
    "\n",
    "\n",
    "visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "visualizer.AddModels(url=get_model_url(robot))\n",
    "\n",
    "visualizer.Run(loop_once=not running_as_notebook)\n",
    "meshcat.DeleteAddedControls()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1bd83adc",
   "metadata": {
    "colab_type": "text",
    "id": "7q0A14bAilIX"
   },
   "source": [
    "# Robot hands\n",
    "\n",
    "I don't have the same smörgåsbord of robot models to offer for robot hands (yet).  We do have the allegro hand model available, and I'm happy to add more here (though many will require some care to handle their kinematic constraints).  For now, you can at least try out the [Allegro Hand](http://www.wonikrobotics.com/Allegro-Hand.htm).\n",
    "\n",
    "TODO(russt): Consider porting Robotiq, Sandia, or IHY-hand from our [openhumanoids](https://github.com/openhumanoids/oh-distro/tree/master/software/models/common_components) project."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3d291baa",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "ILYLouFTjv6e"
   },
   "outputs": [],
   "source": [
    "meshcat.Delete()\n",
    "\n",
    "visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "visualizer.AddModels(\n",
    "    url=\"package://drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf\"\n",
    ")\n",
    "\n",
    "visualizer.Run(loop_once=not running_as_notebook)\n",
    "\n",
    "meshcat.DeleteAddedControls()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7112a8be",
   "metadata": {},
   "source": [
    "# Robots description files from other repositories\n",
    "\n",
    "We've tried to make it very easy to pull in resources from other repositories.  Here's an example of pulling in files directly from github. Drake's MuJoCo parser is the least used/complete of our parsers (I only added a minimal version of it quite recently), so many of these don't parse beautifully yet -- I'm working on it! But it's a good example of how to pull in resources from other repositories, and if there is a particular model you need that doesn't parse well for you, you can let me know."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "074ad941",
   "metadata": {},
   "source": [
    "## MuJoCo Menagerie"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ec925772",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "5SjOClhTltPk"
   },
   "outputs": [],
   "source": [
    "if running_as_notebook:  # I don't want to download the menagerie in CI.\n",
    "    meshcat.Delete()\n",
    "    visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "    visualizer.parser().package_map().AddRemote(\n",
    "        package_name=\"mujoco_menagerie\",\n",
    "        params=PackageMap.RemoteParams(\n",
    "            # This repository doesn't have up-to-date tags/releases; the scary\n",
    "            # hash in the url is the most recent commit sha at the time of my\n",
    "            # writing.\n",
    "            urls=[\n",
    "                f\"https://github.com/google-deepmind/mujoco_menagerie/archive/0c8c9315506dbd4e9b3c1a6ff6faa28612792d1d.tar.gz\"\n",
    "            ],\n",
    "            sha256=(\n",
    "                \"ea7e63453236c1e8583b3dc7acc75d9204ef8b9b8407d75c2c6a0b75e0be202a\"\n",
    "            ),\n",
    "            strip_prefix=\"mujoco_menagerie-0c8c9315506dbd4e9b3c1a6ff6faa28612792d1d/\",\n",
    "        ),\n",
    "    )\n",
    "    visualizer.AddModels(\n",
    "        url=\"package://mujoco_menagerie/kuka_iiwa_14/iiwa14.xml\"\n",
    "    )\n",
    "    visualizer.Run(loop_once=True)\n",
    "    meshcat.DeleteAddedControls()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "edcabaa2",
   "metadata": {},
   "source": [
    "## DeepMind Control Suite"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0adce0ac",
   "metadata": {},
   "outputs": [],
   "source": [
    "if running_as_notebook:  # I don't want to download the dm_control suite in CI.\n",
    "    meshcat.Delete()\n",
    "    visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "    visualizer.parser().package_map().AddRemote(\n",
    "        package_name=\"dm_control\",\n",
    "        params=PackageMap.RemoteParams(\n",
    "            urls=[\n",
    "                f\"https://github.com/google-deepmind/dm_control/archive/refs/tags/1.0.15.tar.gz\"\n",
    "            ],\n",
    "            sha256=(\n",
    "                \"bac091b18689330a99b7c18ddf86baa916527f5e4ab8e3ded0c8caff1dab2048\"\n",
    "            ),\n",
    "            strip_prefix=\"dm_control-1.0.15/\",\n",
    "        ),\n",
    "    )\n",
    "    visualizer.AddModels(url=\"package://dm_control/dm_control/suite/dog.xml\")\n",
    "    plant = visualizer.parser().plant()\n",
    "    visualizer.Run(loop_once=not running_as_notebook)\n",
    "    meshcat.DeleteAddedControls()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cfaf4640",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.10.8 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
